#include "track.h"
namespace perception
{
    namespace algorithm
    {
        Track::Track(track_config &config, rclcpp::Node *private_nh)
            : private_nh_(private_nh)
        {
            this->track_config_ = config;
            this->euclidean_distance = config.euclidean_distance;
            this->max_cluster_size = config.max_cluster_size;
            this->min_cluster_size = config.min_cluster_size;
            km_ = std::make_shared<MatchKM>(private_nh); // NOLINT
            ids.assign(255, false);
        }

        Track::~Track() {}
        int Track::Process(std::vector<LidarObject> &objs, Localization &loc)
        {
            // ？- 未完成
            // 1.坐标转换
            // 1.1 取定位与雷达最接近且满足时间差阈值的定位数据
            // 判断当前时间是否小于上一帧的时间，小于则将上一帧数据全部置空 ？
            // if (pre_time < 0.1)
            //     dt = 0.1;
            // else
            // {
            //     dt = (loc.time - pre_time);
            //     if (pre_time > loc.time)
            //         dt = 0.1;
            //     else if (dt < 0.00001)
            //         dt = 0.1;
            // }
            std::cout << "dt:" << dt << std::endl;
            pre_time = loc.time;
            std::vector<LidarObject> objs_transform;
            objs_transform = objs;
            CoordinateTransformation(objs_transform, loc);

            std::vector<bool> g_matched(objs_transform.size(), false);
            std::vector<bool> c_matched(now_objs.size(), false);
            double euclidean[objs_transform.size()][now_objs.size()];

            // 2.匹配 这里选的还是中心点进行匹配，后面看改成角点或者两个一起加权平均

            for (unsigned int g = 0; g < objs_transform.size(); ++g)
            {
                for (unsigned int c = 0; c < now_objs.size(); ++c)
                {
                    euclidean[g][c] =
                        abs(objs_transform[g].corner_point.x - now_objs[c].cp.x) +
                        abs(objs_transform[g].corner_point.y - now_objs[c].cp.y);
                    // euclidean[g][c] = abs(objs_transform[g].x - now_objs[c].x) +
                    // abs(objs_transform[g].y - now_objs[c].y); euclideanLW[g][c] =
                    // abs(objs_transform[g].length - now_objs[c].length_box) +
                    // abs(objs_transform[g].width - now_objs[c].width_box);
                }
            }

            // Find the smallest euclidean distance and associate if smaller than the
            // threshold
            std::vector<std::pair<int, int>> pairs;
            for (unsigned int c = 0; c < now_objs.size(); ++c)
            {
                unsigned int position;
                double min_distance = euclidean_distance;
                for (unsigned int g = 0; g < objs_transform.size(); ++g)
                {
                    // if (euclidean[g][c] < min_distance && euclideanLW[g][c] < 3)
                    if (euclidean[g][c] < min_distance)
                    {
                        min_distance = euclidean[g][c];
                        position = g;
                    }
                }
                if (min_distance < euclidean_distance)
                {
                    g_matched[position] = true, c_matched[c] = true;
                    pairs.push_back(std::pair<int, int>(c, position));
                }
            }

            // Update Tracked Clusters
            int counter_match = 0;
            for (unsigned int p = 0; p < pairs.size(); ++p)
            {
                counter_match++;
                now_objs[pairs[p].first].track_type_ = track_type::MATCHED;
                // WARN("Update!!! x:{}, y:{},euclidean:{}", now_objs[pairs[p].first].x,
                // now_objs[pairs[p].first].y,euclidean[pairs[p].second][pairs[p].first]);
                now_objs[pairs[p].first].update(objs_transform[pairs[p].second], loc.time, loc);
                // WARN("Update!!! x2:{}, y2:{}", now_objs[pairs[p].first].x,
                // now_objs[pairs[p].first].y);
            }
            // WARN("counter_match :{}", counter_match);
            unsigned int o = 0;
            unsigned int p = now_objs.size();

            // Initialisation of new Cluster Objects
            int counter_failed = 0;
            for (unsigned int i = 0; i < objs_transform.size(); ++i)
            {
                if (g_matched[i] == false &&
                    objs_transform[i].cells.size() < max_cluster_size &&
                    objs_transform[i].cells.size() > min_cluster_size)
                {
                    counter_failed++;
                    Match cl(track_config_);
                    cl.track_type_ = track_type::NEW_OBJ;
                    cl.Init(cclusters, objs_transform[i], loc.time, loc);
                    // cclusters++;
                    // if (cclusters > 31)
                    //     cclusters = 0;
                    now_objs.push_back(cl);
                }
            }
            // WARN("counter_failed :{}", counter_failed);
            // 更新未匹配到的上一帧目标
            for (unsigned int i = 0; i < p; ++i)
            {
                if (c_matched[i] == false)
                {
                    now_objs[i].track_type_ = track_type::MATCHING_FAILED;
                    now_objs[i].update(loc.time, loc);
                }
            }
            // 删除置信度小于0的目标
            for (int i = now_objs.size() - 1; i >= 0; --i)
            {
                if (now_objs[i].confidence < 1 || isnan(now_objs[i].x) ||
                    isnan(now_objs[i].y))
                    now_objs.erase(now_objs.begin() + i);
                if (now_objs[i].confidence > track_config_.max_confidence)
                    now_objs[i].confidence = track_config_.max_confidence;
            }
            // 删除位置相同的目标
            for (int i = 0; i < now_objs.size() - 1; ++i)
            {
                for (int j = i + 1; j < now_objs.size(); ++j)
                {
                    if ((std::fabs(now_objs[i].x - now_objs[j].x) < 0.1 &&
                         std::fabs(now_objs[i].y - now_objs[j].y) < 0.1) ||
                        (std::fabs(now_objs[i].cp.x - now_objs[j].cp.x) < 0.1 &&
                         std::fabs(now_objs[i].cp.y - now_objs[j].cp.y) < 0.1))
                    {
                        now_objs.erase(now_objs.begin() + j);
                        continue;
                    }
                    // if (std::fabs(now_objs[i].length_box - now_objs[j].length_box) < 0.01
                    // && std::fabs(now_objs[i].width_box - now_objs[j].width_box) < 0.01)
                    // {
                    //     now_objs.erase(now_objs.begin() + j);
                    // }
                }
            }

            return 0;
        }
        int Track::Process2(std::vector<LidarObject> &objs, Localization &loc)
        {
            // ？- 未完成
            // 1.坐标转换
            // 1.1 取定位与雷达最接近且满足时间差阈值的定位数据
            // 判断当前时间是否小于上一帧的时间，小于则将上一帧数据全部置空 ？
            if (pre_time < 0.1)
                dt = 0.1;
            else
            {
                dt = (loc.time - pre_time);
                if (pre_time > loc.time)
                    dt = 0.1;
                else if (dt < 0.001)
                {
                    RCLCPP_WARN(private_nh_->get_logger(), "det time is too low:%f", dt);
                    return 1;
                }
            }
            pre_time = loc.time;
            std::vector<LidarObject> objs_transform;
            objs_transform = objs;
            // RCLCPP_INFO(private_nh_->get_logger(), "1111");
            CoordinateTransformation(objs_transform, loc);
            // RCLCPP_INFO(private_nh_->get_logger(), "2222");
            if (!now_objs_center.empty())
            {
                // 不需要预测,因为更新的时候做了
                // for (auto obj : now_objs_center)
                //     obj.predict();
                std::vector<bool> g_matched(objs_transform.size(), false);
                std::vector<bool> c_matched(now_objs_center.size(), false);
                std::vector<std::pair<int, int>> pairs;

                // km_->Process(objs_transform, now_objs_center, this->euclidean_distance,
                //              g_matched, c_matched, pairs);
                MatchHM(objs_transform, now_objs_center, this->euclidean_distance,
                        g_matched, c_matched, pairs);

                // Update Tracked Clusters
                int counter_match = 0;
                for (unsigned int p = 0; p < pairs.size(); ++p)
                {
                    std::cout << "pairs[p].first:" << pairs[p].first << " pairs[p].second:" << pairs[p].second << std::endl;
                    counter_match++;
                    now_objs_center[pairs[p].first].track_type_ = track_type::MATCHED;
                    now_objs_center[pairs[p].first].update(objs_transform[pairs[p].second],
                                                           dt, loc);
                }
                // RCLCPP_INFO(private_nh_->get_logger(), "8888");

                // 再加一次匹配 两边没匹配上的再匹配一次
                std::vector<LidarObject> objs_transform_tmp;
                for (unsigned int i = 0; i < objs_transform.size(); ++i)
                {
                    if (g_matched[i] == false)
                    {
                        objs_transform_tmp.emplace_back(objs_transform[i]);
                    }
                }
                std::vector<MatchCenter> now_objs_center_tmp;
                for (unsigned int i = 0; i < now_objs_center.size(); ++i)
                {
                    if (c_matched[i] == false)
                    {
                        now_objs_center_tmp.emplace_back(now_objs_center[i]);
                    }
                }
                // 如果新数据都被匹配
                // if (objs_transform_tmp.empty() || now_objs_center_tmp.empty())
                if (1)
                {
                    unsigned int p = now_objs_center.size();
                    // Initialisation of new Cluster Objects
                    for (unsigned int i = 0; i < objs_transform.size(); ++i)
                    {
                        if (g_matched[i] == false)
                        {
                            MatchCenter cl(track_config_);
                            cl.track_type_ = track_type::NEW_OBJ;
                            while (ids[cclusters])
                            {
                                cclusters++;
                                if (cclusters > 254)
                                    cclusters = 0;
                            }
                            cl.Init(cclusters, objs_transform[i], dt, loc);
                            ids[cclusters] = true;
                            cclusters++;
                            now_objs_center.push_back(cl);
                        }
                    }
                    // 更新未匹配到的上一帧目标
                    for (unsigned int i = 0; i < p; ++i)
                    {
                        if (c_matched[i] == false)
                        {
                            now_objs_center[i].track_type_ = track_type::MATCHING_FAILED;
                            now_objs_center[i].update(dt, loc);
                        }
                    }
                }

                else
                {
                    std::vector<bool> g_matched_tmp(objs_transform_tmp.size(), false);
                    std::vector<bool> c_matched_tmp(now_objs_center_tmp.size(), false);
                    std::vector<std::pair<int, int>> pairs_tmp;
                    MatchHM(objs_transform_tmp, now_objs_center_tmp, this->euclidean_distance,
                            g_matched_tmp, c_matched_tmp, pairs_tmp);
                    for (unsigned int p = 0; p < pairs_tmp.size(); ++p)
                    {
                        now_objs_center[pairs_tmp[p].first].track_type_ = track_type::MATCHED;
                        now_objs_center[pairs_tmp[p].first].update(objs_transform_tmp[pairs_tmp[p].second],
                                                                   dt, loc);
                    }
                    for (unsigned int i = 0; i < objs_transform_tmp.size(); ++i)
                    {
                        if (g_matched_tmp[i] == false)
                        {
                            MatchCenter cl(track_config_);
                            cl.track_type_ = track_type::NEW_OBJ;
                            while (ids[cclusters])
                            {
                                cclusters++;
                                if (cclusters > 254)
                                    cclusters = 0;
                            }
                            cl.Init(cclusters, objs_transform_tmp[i], dt, loc);
                            ids[cclusters] = true;
                            cclusters++;
                            // if (cclusters > 254)
                            //     cclusters = 0;
                            now_objs_center.push_back(cl);
                        }
                    }
                    // 更新未匹配到的上一帧目标
                    for (unsigned int i = 0; i < now_objs_center_tmp.size(); ++i)
                    {
                        if (c_matched_tmp[i] == false)
                        {
                            now_objs_center[i].track_type_ = track_type::MATCHING_FAILED;
                            now_objs_center[i].update(dt, loc);
                        }
                    }
                }
                // old
                // Initialisation of new Cluster Objects
                // auto p = now_objs_center.size();
                // for (unsigned int i = 0; i < objs_transform.size(); ++i)
                // {
                //     if (g_matched[i] == false)
                //     {
                //         MatchCenter cl(track_config_);
                //         cl.track_type_ = track_type::NEW_OBJ;
                //         while (ids[cclusters])
                //         {
                //             cclusters++;
                //             if (cclusters > 254)
                //                 cclusters = 0;
                //         }
                //         cl.Init(cclusters, objs_transform[i], dt, loc);
                //         ids[cclusters] = true;
                //         cclusters++;
                //         now_objs_center.push_back(cl);
                //     }
                // }
                // // 更新未匹配到的上一帧目标
                // for (unsigned int i = 0; i < p; ++i)
                // {
                //     if (c_matched[i] == false)
                //     {
                //         now_objs_center[i].track_type_ = track_type::MATCHING_FAILED;
                //         now_objs_center[i].update(dt, loc);
                //     }
                // }
                // 删除置信度小于0的目标
                for (int i = now_objs_center.size() - 1; i >= 0; --i)
                {
                    if (now_objs_center[i].confidence < 1 || isnan(now_objs_center[i].x) ||
                        isnan(now_objs_center[i].y))
                    {
                        std::cout << "<0 id:" << now_objs_center[i].id
                                  << " x:" << now_objs_center[i].x
                                  << " y:" << now_objs_center[i].y
                                  << " this->age:" << now_objs_center[i].age
                                  << " this->confidence:" << now_objs_center[i].confidence
                                  << " heading:" << now_objs_center[i].psi
                                  << std::endl;
                        ids[now_objs_center[i].id] = false;
                        now_objs_center.erase(now_objs_center.begin() + i);
                    }
                    if (now_objs_center[i].confidence > track_config_.max_confidence)
                        now_objs_center[i].confidence = track_config_.max_confidence;
                }
                // RCLCPP_INFO(private_nh_->get_logger(), "6666");
                // 删除位置相同的目标
                for (int i = 0; i < now_objs_center.size() - 1; ++i)
                {
                    for (int j = i + 1; j < now_objs_center.size(); ++j)
                    {
                        if ((std::fabs(now_objs_center[i].x - now_objs_center[j].x) < 0.1 &&
                             std::fabs(now_objs_center[i].y - now_objs_center[j].y) < 0.1))
                        //  ||
                        // (std::fabs(now_objs_center[i].cp.x - now_objs_center[j].cp.x) <
                        //      0.1 &&
                        //  std::fabs(now_objs_center[i].cp.y - now_objs_center[j].cp.y) <
                        //      0.1))
                        {
                            now_objs_center.erase(now_objs_center.begin() + j);
                            ids[now_objs_center[j].id] = false;
                            continue;
                        }
                    }
                }
                //
                ids.clear();
                ids.assign(255, false);
                for (int i = 0; i < now_objs_center.size() - 1; ++i)
                {
                    ids[now_objs_center[i].id] = true;
                }
            }
            else
            {
                for (unsigned int i = 0; i < objs_transform.size(); ++i)
                {
                    MatchCenter cl(track_config_);
                    cl.track_type_ = track_type::NEW_OBJ;
                    while (ids[cclusters])
                    {
                        cclusters++;
                        if (cclusters > 254)
                            cclusters = 0;
                    }
                    cl.Init(cclusters, objs_transform[i], dt, loc);
                    ids[cclusters] = true;
                    cclusters++;
                    now_objs_center.push_back(cl);
                }
            }

            return 0;
        }
        void Track::CoordinateTransformation(std::vector<LidarObject> &cells,
                                             Localization &loc)
        {
            float vx = loc.speed * std::cos(loc.angle * deg2rad);
            float vy = loc.speed * std::sin(loc.angle * deg2rad);
            // std::cout << "vx:" << vx << " vy:" << vy << std::endl;
            auto tmp = cells;
            for (size_t i = 0; i < cells.size(); i++)
            {
                cells[i].heading = -loc.angle * deg2rad + cells[i].heading;
                cells[i].x = tmp[i].x * std::cos(-loc.angle * deg2rad) +
                             tmp[i].y * std::sin(-loc.angle * deg2rad) + loc.xg -
                             vx * dt * 1;
                cells[i].y = tmp[i].y * std::cos(-loc.angle * deg2rad) -
                             tmp[i].x * std::sin(-loc.angle * deg2rad) + loc.yg -
                             vy * dt * 1;
                cells[i].corner_point.x =
                    tmp[i].corner_point.x * std::cos(-loc.angle * deg2rad) +
                    tmp[i].corner_point.y * std::sin(-loc.angle * deg2rad) + loc.xg -
                    vx * dt * 1;
                cells[i].corner_point.y =
                    tmp[i].corner_point.y * std::cos(-loc.angle * deg2rad) -
                    tmp[i].corner_point.x * std::sin(-loc.angle * deg2rad) + loc.yg -
                    vy * dt * 1;
                for (auto &cell : cells[i].cells)
                {
                    auto tmp_cell = cell;
                    cell.x = tmp_cell.x * std::cos(-loc.angle * deg2rad) +
                             tmp_cell.y * std::sin(-loc.angle * deg2rad) + loc.xg -
                             vx * dt * 1;
                    cell.y = tmp_cell.y * std::cos(-loc.angle * deg2rad) -
                             tmp_cell.x * std::sin(-loc.angle * deg2rad) + loc.yg -
                             vy * dt * 1;
                }
                if (cells[i].id > 300)
                {
                    RCLCPP_ERROR(private_nh_->get_logger(), "error id :%d:", cells[i].id);
                }
            }
        }
        void Track::MatchHM(std::vector<LidarObject> &input_objs,
                            std::vector<MatchCenter> &now_objs, float &euclidean_distance,
                            std::vector<bool> &g_matched,
                            std::vector<bool> &c_matched,
                            std::vector<std::pair<int, int>> &pairs)
        {
            std::vector<std::vector<double>> cost_matrix;
            std::cout << "input_objs size:" << input_objs.size() << " now_objs.size():" << now_objs.size() << std::endl;
            if (input_objs.size() < now_objs.size())
            {
                std::cout << "cost_matrix:" << std::endl;
                for (int g = 0; g < input_objs.size(); ++g)
                {
                    std::vector<double> tmp;
                    for (int c = 0; c < now_objs.size(); ++c)
                    {
                        double dis = ((std::fabs(input_objs[g].x - now_objs[c].x) +
                                       std::fabs(input_objs[g].y - now_objs[c].y)));
                        if (dis > euclidean_distance)
                            dis = euclidean_distance * 10;
                        tmp.emplace_back(dis);
                        std::cout << dis << ",";
                    }
                    std::cout << "\n";
                    cost_matrix.emplace_back(tmp);
                }
                HungarianAlgorithm HungAlgo;
                std::vector<int> assignment;
                double cost = HungAlgo.Solve(cost_matrix, assignment);
                for (unsigned int x = 0; x < cost_matrix.size(); x++)
                {

                    std::cout << "~~~~~\n";
                    std::cout << "x:" << x << " [assignment[x]]:" << assignment[x] << std::endl;
                    std::cout << "input_objs[x] id:" << input_objs[x].id << " " << input_objs[x].x << " " << input_objs[x].y << std::endl;
                    std::cout << "now_objs[assignment[x]] id:" << now_objs[assignment[x]].id << " " << now_objs[assignment[x]].x << " " << now_objs[assignment[x]].y << std::endl;
                    std::cout << "cost_matrix[x][assignment[x]]:" << cost_matrix[x][assignment[x]] << std::endl;
                    // euclidean_distance = std::sqrt(now_objs[assignment[x]].cvx * now_objs[assignment[x]].cvx + now_objs[assignment[x]].cvy * now_objs[assignment[x]].cvy) * 0.1 + 10;
                    if (cost_matrix[x][assignment[x]] < static_cast<double>(euclidean_distance))
                    {
                        g_matched[x] = true;
                        c_matched[assignment[x]] = true;
                        std::pair<int, int> p;
                        p.first = assignment[x];
                        p.second = x;
                        pairs.emplace_back(p);
                    }
                }
            }
            else
            {
                std::cout << "cost_matrix:" << std::endl;
                for (int c = 0; c < now_objs.size(); ++c)
                {
                    std::vector<double> tmp;
                    for (int g = 0; g < input_objs.size(); ++g)
                    {
                        double dis = ((std::fabs(input_objs[g].x - now_objs[c].x) +
                                       std::fabs(input_objs[g].y - now_objs[c].y)));
                        if (dis > euclidean_distance)
                            dis = euclidean_distance * 10;
                        tmp.emplace_back(dis);
                        std::cout << dis << ",";
                    }
                    cost_matrix.emplace_back(tmp);
                    std::cout << "\n";
                }
                HungarianAlgorithm HungAlgo;
                std::vector<int> assignment;
                double cost = HungAlgo.Solve(cost_matrix, assignment);
                for (unsigned int x = 0; x < cost_matrix.size(); x++)
                {
                    std::cout << "~~~~~\n";
                    std::cout << "x:" << x << " [assignment[x]]:" << assignment[x] << std::endl;
                    std::cout << "input_objs[assignment[x]] id:" << input_objs[assignment[x]].id << " " << input_objs[assignment[x]].x << " " << input_objs[assignment[x]].y << std::endl;
                    std::cout << "now_objs x id:" << now_objs[x].id << " " << now_objs[x].x << " " << now_objs[x].y << std::endl;
                    std::cout << "cost_matrix[x][assignment[x]]:" << cost_matrix[x][assignment[x]] << std::endl;
                    // euclidean_distance = std::sqrt(now_objs[assignment[x]].cvx * now_objs[x].cvx + now_objs[x].cvy * now_objs[x].cvy) * 0.1 + 10;
                    if (cost_matrix[x][assignment[x]] < static_cast<double>(euclidean_distance))
                    {
                        g_matched[assignment[x]] = true;
                        c_matched[x] = true;
                        std::pair<int, int> p;
                        p.first = x;
                        p.second = assignment[x];
                        pairs.emplace_back(p);
                    }
                }
            }

            // std::cout << "\ncost: " << cost << std::endl;
        }
    } // namespace algorithm
} // namespace perception